

#include "phys_solve_math.h"

/*

二维行列式

| a  b |
| c  d |

二维方程组

 a_11 * x + a_12 * y = b_1
 a_21 * x + a_22 * y = b_2

*/
namespace phys_engine
{
    double point_distance(simulate_point _p1,simulate_point _p2)
    {
        return std::sqrt(std::pow(_p1.x-_p2.x,2)+std::pow(_p1.y-_p2.y,2));
    }

    double point_distance(double _x1,double _y1,double _x2,double _y2)
    {
        return std::sqrt(std::pow(_x1-_x2,2)+std::pow(_y1-_y2,2));
    }

    double clac_angle(simulate_point _p1,simulate_point _p2)
    {
        return std::atan2(_p2.y-_p1.y,_p2.x-_p1.x);
    }

    double calc_angle(double _x1,double _y1,double _x2,double _y2)
    {
        return std::atan2(_y2-_y1,_x2-_x1);
    }

    double _2ddet(double a,double b,double c,double d)
    {
        return a*d-b*c;
    }

    double solve_linear_x(double a_11,double a_12,double b_1,double a_21,double a_22,double b_2)
    {
        return _2ddet(b_1,a_12,b_2,a_22)/_2ddet(a_11,a_12,a_21,a_22);
    }

    double solve_linear_y(double a_11,double a_12,double b_1,double a_21,double a_22,double b_2)
    {
        return _2ddet(a_11,b_1,a_21,b_2)/_2ddet(a_11,a_12,a_21,a_22);
    }

    double calc_angacc(double f_effx,double f_effy,double f_divx,double f_divy,double inertia)
    {
        return (f_effx*f_divy-f_effy*f_divx)/inertia;
    }

    simulate_point calc_point(simulate_point _p,double angle,double dist)
    {
        return simulate_point(_p.x+dist*std::cos(angle),_p.y+dist*std::sin(angle));
    }

    simulate_point calc_point(double x,double y,double angle,double dist)
    {
        return simulate_point(x+dist*std::cos(angle),y+dist*std::sin(angle));
    }
}
